/*****************************************************************************
*                                                                            *
*  OpenNI 2.x Alpha                                                          *
*  Copyright (C) 2012 PrimeSense Ltd.                                        *
*                                                                            *
*  This file is part of OpenNI.                                              *
*                                                                            *
*  Licensed under the Apache License, Version 2.0 (the "License");           *
*  you may not use this file except in compliance with the License.          *
*  You may obtain a copy of the License at                                   *
*                                                                            *
*      http://www.apache.org/licenses/LICENSE-2.0                            *
*                                                                            *
*  Unless required by applicable law or agreed to in writing, software       *
*  distributed under the License is distributed on an "AS IS" BASIS,         *
*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  *
*  See the License for the specific language governing permissions and       *
*  limitations under the License.                                            *
*                                                                            *
*****************************************************************************/
#include <OpenNI.h>
//#include <Driver/OniFile/PlayerDevice.h>
#include "Viewer.h"
#include <iostream>
using namespace std;

openni::PlaybackControl* _pPlaybackControl;


int main(int argc, char** argv)
{
	openni::Status rc = openni::STATUS_OK;

	openni::Device device;
	//oni_file::PlayerDevice player("C:\\Downloads\\test.oni");
	openni::VideoStream depth, color;
	const char* deviceURI = openni::ANY_DEVICE;
	if (argc > 1)
	{
		deviceURI = argv[1];
	}

	rc = openni::OpenNI::initialize();

	printf("After initialization:\n%s\n", openni::OpenNI::getExtendedError());

	rc = device.open(deviceURI);	// if deviceURI points to a "*.oni" file, then open it. 
	if (rc != openni::STATUS_OK)
	{
		printf("SimpleViewer: Device open failed:\n%s\n", openni::OpenNI::getExtendedError());
		openni::OpenNI::shutdown();
		return 1;
	}

	_pPlaybackControl = device.getPlaybackControl();

	//print supported sensor format
	const openni::Array<openni::VideoMode>& color_modes = device.getSensorInfo( openni::SENSOR_COLOR )->getSupportedVideoModes();
	cout << " Color" << endl; 
	for (int i=0; i<color_modes.getSize();i++) {
		cout<< "FPS: " << color_modes[i].getFps() << " Pixel format: " << color_modes[i].getPixelFormat() << " X resolution: " << color_modes[i].getResolutionX() << " Y resolution: " << color_modes[i].getResolutionY() << endl;
	}
	const openni::Array<openni::VideoMode>& depth_modes = device.getSensorInfo( openni::SENSOR_DEPTH )->getSupportedVideoModes();
	cout << " Depth" << endl; 
	for (int i=0; i<depth_modes.getSize();i++) {
		cout<< "FPS: " << depth_modes[i].getFps() << " Pixel format: " << depth_modes[i].getPixelFormat() << " X resolution: " << depth_modes[i].getResolutionX() << " Y resolution: " << depth_modes[i].getResolutionY() << endl;
	}

	rc = depth.create(device, openni::SENSOR_DEPTH);

	if (rc == openni::STATUS_OK)
	{
		openni::VideoMode depthMode = depth.getVideoMode();
		depthMode.setFps(30);
		depthMode.setResolution(640,480);
		depthMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
		if (argc <= 1)
			rc = depth.setVideoMode(depthMode); 
		if(rc == openni::STATUS_OK){
			rc = depth.start();
		}
		if (rc != openni::STATUS_OK)
		{
			printf("SimpleViewer: Couldn't start depth stream:\n%s\n", openni::OpenNI::getExtendedError());
			depth.destroy();
		}
	}
	else
	{
		printf("SimpleViewer: Couldn't find depth stream:\n%s\n", openni::OpenNI::getExtendedError());
	}

	rc = color.create(device, openni::SENSOR_COLOR);
	if (rc == openni::STATUS_OK)
	{
		openni::VideoMode colorMode = color.getVideoMode();
		colorMode.setFps(30);
		colorMode.setResolution(640,480);
		colorMode.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
		if (argc <= 1)
			rc = color.setVideoMode(colorMode);
		if( rc == openni::STATUS_OK){
			rc = color.start();
		}
		if (rc != openni::STATUS_OK)
		{
			printf("SimpleViewer: Couldn't start color stream:\n%s\n", openni::OpenNI::getExtendedError());
			color.destroy();
		}
	}
	else
	{
		printf("SimpleViewer: Couldn't find color stream:\n%s\n", openni::OpenNI::getExtendedError());
	}

	if (!depth.isValid() || !color.isValid())
	{
		printf("SimpleViewer: No valid streams. Exiting\n");
		openni::OpenNI::shutdown();
		return 2;
	}


	int changedIndex;
	openni::VideoStream* streams[] = {&depth, &color};
	rc = openni::OpenNI::waitForAnyStream(streams, 2, &changedIndex);
	if (rc != openni::STATUS_OK)
	{
		printf("Wait failed\n");
	}

	openni::VideoFrameRef		m_depthFrame;
	openni::VideoFrameRef		m_colorFrame;

	switch (changedIndex)
	{
	case 0:
		depth.readFrame(&m_depthFrame); break;
	case 1:
		color.readFrame(&m_colorFrame); break;
	default:
		printf("Error in wait\n");
	}

	openni::Recorder recRecorder;
	recRecorder.create( "c:\\Downloads\\test1.oni" );
	recRecorder.attach( depth );
	recRecorder.attach( color );

	recRecorder.start();

	SampleViewer sampleViewer("Simple Viewer", device, depth, color);

	rc = sampleViewer.init(argc, argv);
	if (rc != openni::STATUS_OK)
	{
		openni::OpenNI::shutdown();
		return 3;
	}
	sampleViewer.run();
}